#include "SAW.h"

#include <ctime>
#include <iostream>
#include <algorithm>

SAW::SAW(unsigned long* stepSizes, size_t amountStepSizes, unsigned int seed){
	this->directions = 8;
    this->stepSizes = stepSizes;
    this->amountStepSizes = amountStepSizes;
    this->possibleDirections = new bool[directions];
    srand(time(NULL));
}

unsigned long SAW::newStepSize() {
	unsigned long result = stepSizes[stepSizeCounter];
    stepSizeCounter = (stepSizeCounter + 1) % amountStepSizes;
    return result;
}

unsigned long SAW::walk() {
    stepSizeCounter = 0;
    stepSize = newStepSize();
	path.reserve(5000000);
    Point oldPoint = {startX, startY, startZ, startU};
    path.push_back(oldPoint);
    Point newPoint;
    unsigned long steps = 0;

    unsigned long direction;
    unsigned long directions = calculatePossibleDirections(oldPoint);
    while (directions != 0) {
    	direction = getDirection((rand() % directions));
        if (direction == 0) {
            newPoint.x = oldPoint.x;
            newPoint.y = oldPoint.y + stepSize;
            newPoint.z = oldPoint.z;
            newPoint.u = oldPoint.u;
        } else if (direction == 1) {
            newPoint.x = oldPoint.x + stepSize;
            newPoint.y = oldPoint.y;
            newPoint.z = oldPoint.z;
            newPoint.u = oldPoint.u;
        } else if (direction == 2) {
            newPoint.x = oldPoint.x;
            newPoint.y = oldPoint.y - stepSize;
            newPoint.z = oldPoint.z;
            newPoint.u = oldPoint.u;
        }else if (direction == 3) {
            newPoint.x = oldPoint.x - stepSize;
            newPoint.y = oldPoint.y;
            newPoint.z = oldPoint.z;
            newPoint.u = oldPoint.u;
        } else if(direction == 4){
            newPoint.x = oldPoint.x;
            newPoint.y = oldPoint.y;
            newPoint.z = oldPoint.z + 1;
            newPoint.u = oldPoint.u;
        } else if(direction == 5){
            newPoint.x = oldPoint.x;
            newPoint.y = oldPoint.y;
            newPoint.z = oldPoint.z - 1;
            newPoint.u = oldPoint.u;
        }else if (direction == 6) {
         	newPoint.x = oldPoint.x;
            newPoint.y = oldPoint.y;
            newPoint.z = oldPoint.z;
            newPoint.u = oldPoint.u + 1;
        } else if (direction == 7) {
        	 newPoint.x = oldPoint.x;
            newPoint.y = oldPoint.y;
            newPoint.z = oldPoint.z;
            newPoint.u = oldPoint.u - 1;
        }
        ++steps;
        doStep(oldPoint, newPoint);
        oldPoint = newPoint;
        directions = calculatePossibleDirections(oldPoint);
        if(steps % 10000 == 0){
        	std::cout << "Steps: " << steps << std::endl;
        }
    }

    path.clear();
    return steps;
}

bool SAW::canGoNord(Point point) {
    for (size_t i = 1; i <= stepSize; ++i) {
        if (pathContains(Point{point.x, point.y + i, point.z, point.u})) {
            return false;
        }
    }
    return true;
}

bool SAW::canGoEast(Point point) {
    for (size_t i = 1; i <= stepSize; ++i) {
        if (pathContains(Point{point.x + i, point.y, point.z, point.u})) {
            return false;
        }
    }
    return true;
}

bool SAW::canGoSouth(Point point) {
    for (size_t i = 1; i <= stepSize; ++i) {
        if (pathContains(Point{point.x, point.y - i, point.z, point.u})) {
            return false;
        }
    }
    return true;
}

bool SAW::canGoWest(Point point) {
    for (size_t i = 1; i <= stepSize; ++i) {
        if (pathContains(Point{point.x - i, point.y, point.z, point.u})) {
            return false;
        }
    }
    return true;
}

bool SAW::canGoUp(Point point) {
    for (size_t i = 1; i <= stepSize; ++i) {
        if (pathContains(Point{point.x, point.y, point.z+i, point.u})) {
            return false;
        }
    }
    return true;
}

bool SAW::canGoDown(Point point) {
    for (size_t i = 1; i <= stepSize; ++i) {
        if (pathContains(Point{point.x, point.y, point.z-i, point.u})) {
            return false;
        }
    }
    return true;
}

bool SAW::canGoUp4(Point point) {
    for (size_t i = 1; i <= stepSize; ++i) {
        if (pathContains(Point{point.x, point.y, point.z, point.u+i})) {
            return false;
        }
    }
    return true;
}

bool SAW::canGoDown4(Point point) {
    for (size_t i = 1; i <= stepSize; ++i) {
        if (pathContains(Point{point.x, point.y, point.z, point.u-i})) {
            return false;
        }
    }
    return true;
}

unsigned long SAW::calculatePossibleDirections(Point point) {
    unsigned long counter = 0;

    possibleDirections[0] = canGoNord(point);
    possibleDirections[1] = canGoEast(point);
    possibleDirections[2] = canGoSouth(point);
    possibleDirections[3] = canGoWest(point);
    possibleDirections[4] = canGoUp(point);
    possibleDirections[5] = canGoDown(point);
    possibleDirections[6] = canGoUp4(point);
    possibleDirections[7] = canGoDown4(point);


    for(size_t i=0; i<directions; ++i){
        if(possibleDirections[i]){
            ++counter;
        }
    }   

    return counter;
}

unsigned long SAW::getDirection(unsigned int number) {
    size_t counter = 0;

    for (size_t i = 0; i < directions; ++i) {
        if (number == counter && possibleDirections[i]) {
            return i;
        }
        if (possibleDirections[i]) {
            ++counter;
        }
    }
}


bool cmp(SAW::Point p1, SAW::Point p2) {
    //return (p1.x != p2.x) ? (p1.x < p2.x) : (p1.y < p2.y);//TODO hoe kan je iets 3D's vergelijken
    if(p1.x != p2.x){
        return p1.x < p2.x;
    } else if(p1.y != p2.y){
        return p1.y < p2.y;
    } else if(p1.z != p2.z){
        return p1.z < p2.z;
    } else {
    	return p1.u < p2.u;
    }
}

bool SAW::pathContains(Point point) {
    return std::binary_search(path.begin(), path.end(), point, cmp); 
}


void SAW::doStepNord(Point oldPoint, Point newPoint){
    for (long i = oldPoint.y; i < newPoint.y; ++i) {
    	std::vector<Point>::iterator it = std::lower_bound(path.begin(), path.end(), Point {oldPoint.x, i, oldPoint.z, oldPoint.u}, cmp);
		path.insert(it, Point{oldPoint.x, i, oldPoint.z, oldPoint.u});
	}
}

void SAW::doStepSouth(Point oldPoint, Point newPoint){
    for (long i = newPoint.y; i < oldPoint.y; ++i) {
        std::vector<Point>::iterator it = std::lower_bound(path.begin(), path.end(), Point {oldPoint.x, i, oldPoint.z, oldPoint.u}, cmp);
        path.insert(it, Point{oldPoint.x, i, oldPoint.z, oldPoint.u});
    }
}

void SAW::doStepEast(Point oldPoint, Point newPoint){
    for (long i = oldPoint.x + 1; i <= newPoint.x; ++i) {
        std::vector<Point>::iterator it = std::lower_bound(path.begin(), path.end(), Point {i, oldPoint.y, oldPoint.z, oldPoint.u}, cmp);
        path.insert(it, Point{i, oldPoint.y, oldPoint.z, oldPoint.u});
    }
}

void SAW::doStepWest(Point oldPoint, Point newPoint){
    for (long i = newPoint.x; i < oldPoint.x; ++i) {
        std::vector<Point>::iterator it = std::lower_bound(path.begin(), path.end(), Point {i, oldPoint.y, oldPoint.z, oldPoint.u}, cmp);
        path.insert(it, Point{i, oldPoint.y, oldPoint.z, oldPoint.u});
    }
}

void SAW::doStepUp(Point oldPoint, Point newPoint){
    for (long i = oldPoint.z + 1; i <= newPoint.z; ++i) {
        std::vector<Point>::iterator it = std::lower_bound(path.begin(), path.end(), Point {oldPoint.x, oldPoint.y, i, oldPoint.u}, cmp);
        path.insert(it, Point{oldPoint.x, oldPoint.y, i, oldPoint.u});
    }
}

void SAW::doStepDown(Point oldPoint, Point newPoint){
    for (long i = newPoint.z; i < oldPoint.z; ++i) {
        std::vector<Point>::iterator it = std::lower_bound(path.begin(), path.end(), Point {oldPoint.x, oldPoint.y, i, oldPoint.u}, cmp);
        path.insert(it, Point{oldPoint.x, oldPoint.y, i, oldPoint.u});
    }
}
void SAW::doStepUp4(Point oldPoint, Point newPoint){
    for (long i = oldPoint.u + 1; i <= newPoint.u; ++i) {
        std::vector<Point>::iterator it = std::lower_bound(path.begin(), path.end(), Point {oldPoint.x, oldPoint.y, oldPoint.z, i}, cmp);
        path.insert(it, Point{oldPoint.x, oldPoint.y, oldPoint.z, i});
    }
}

void SAW::doStepDown4(Point oldPoint, Point newPoint){
    for (long i = newPoint.u; i < oldPoint.u; ++i) {
        std::vector<Point>::iterator it = std::lower_bound(path.begin(), path.end(), Point {oldPoint.x, oldPoint.y, oldPoint.z, i}, cmp);
        path.insert(it, Point{oldPoint.x, oldPoint.y, oldPoint.z, i});
    }
}

void SAW::doStep(Point oldPoint, Point newPoint) {
    if(oldPoint.x == newPoint.x){
        if(oldPoint.y == newPoint.y){
        	if(oldPoint.u == newPoint.u){// stap ergens in de z richting
		        if(oldPoint.z < newPoint.z){
		            doStepUp(oldPoint, newPoint);
		        } else {
		            doStepDown(oldPoint, newPoint);
		        }
        	} else{ // stap ergens in de u richting
		        if(oldPoint.u < newPoint.u){
		            doStepUp4(oldPoint, newPoint);
		        } else {
		            doStepDown4(oldPoint, newPoint);
		        }
        	}
        } else { // stap ergens in de y richting
            if(oldPoint.y < newPoint.y){
                doStepNord(oldPoint, newPoint);
            } else {
                doStepSouth(oldPoint, newPoint);
            }
        }
    } else { // stap ergens in de x richting
        if(oldPoint.x < newPoint.x){
            doStepEast(oldPoint, newPoint);
        } else {
            doStepWest(oldPoint, newPoint);
        }
    }
   
    stepSize = newStepSize();
}
